package it.trekker.step;

import java.util.ArrayList;
import java.util.List;

import it.trekker.sensors.AccelerometerCoord;
import it.trekker.sensors.IAccelerometerListener;

public class StepDetector implements IAccelerometerListener {
	
	private List<IStepDetectorListener> stepDetectorListeners = null;
	
	private long k = 0;
	private long i = k+1;
//	private float accZ = 0;
	private float max = 0;
	private double th = 0.0;
//	private static double alpha = 52.8687;	// constant
	private static double alpha = 0.8;	// constant
	private static final double beta = 3.1541;	// constant
	
	public StepDetector() {
		stepDetectorListeners = new ArrayList<IStepDetectorListener>();
	}
	
	public double getAlpha() {
		return alpha;
	}

	public void setAlpha(double alpha) {
		StepDetector.alpha = alpha;
	}
	
	@Override
	public void updateAcceleration(float acceleration) {
		
		detectStep(acceleration);
		
	}

	public void addListener(IStepDetectorListener listener) {
		stepDetectorListeners.add(listener);
	}
	
	public void removeListener(IStepDetectorListener listener) {
		stepDetectorListeners.remove(listener);
	}

	@Override
	public void updateAccelCoord(AccelerometerCoord coord) {
//		detectStep(coord.getZ_axis());
	}
	
	
	private void initParameters(float accZ) {
		
		// TODO
		
	}
	
	

	private void detectStep(float accZ) {
		
		th = alpha / (i-k) + beta;
		
		double diff = Math.abs(max - accZ);
		if(diff >= th) {	// step detected
			k = i;
			max = accZ;
			
			for(IStepDetectorListener listener : stepDetectorListeners) {
				listener.onStep();
			}
		}
		else {
			if(accZ > max) {
				max = accZ;
			}
		}
		
		i++;
		
		// Reset to avoid overflow
		if(i == Long.MAX_VALUE) {
			long iMinusK = i-k;
			k = 0;
			i = iMinusK;
		}
	}
}
